import os
import time

import cv2
import numpy as np
import open3d as o3d
import json
import matplotlib.pyplot as plt

# import openpyxl

RADIO = 80
MEASURE_UNIT = 'CM'
import utils.detection_3d.calculate_utils as calculate_utils

# import calculate_utils

calculate_cfg = "utils/detection_3d/calculate_cfg.json"
with open(calculate_cfg, 'r') as f:
    config = json.load(f)
    CAMERA_LENGTH = float(config['x_step_side'])
    CAMERA_WIDTH = float(config['y_step_side'])

CAMERA_HEIGHT = 1  # 线扫单位高度 （毫米）
V_RADIO = 1000 * 1000 * 1  # 转化为立方毫米的比例
CONSTANT_SIDE = CAMERA_WIDTH * CAMERA_HEIGHT * CAMERA_LENGTH / V_RADIO  # 求和之后乘的系数

"""
def write_vol(pin1, pin2, pin3):
    data = openpyxl.load_workbook(vol_folder)
    stn = data.sheetnames[0]
    # sheetname = data.get_sheet_names
    table = data.get_sheet_by_name(stn)
    table = data.active
    nrows = table.max_row
    table.cell(nrows + 1, 1).value = filename
    table.cell(nrows + 1, 2).value = pin1
    table.cell(nrows + 1, 3).value = pin2
    table.cell(nrows + 1, 4).value = pin3
    data.save(vol_folder)


"""

"""
def write_slice(pin_total, pin_up, pin_down, pin_depth):
    wb = openpyxl.Workbook()
    ws = wb.active
    ws.cell(1, 1).value = "pin_depth"
    ws.cell(1, 2).value = "pin_total"
    ws.cell(1, 3).value = "pin_up"
    ws.cell(1, 4).value = "pin_down"
    length = len(pin_total)
    for i in range(length):
        ws.cell(2 + i, 1).value = pin_depth[i]
        ws.cell(2 + i, 2).value = pin_total[i]
        ws.cell(2 + i, 3).value = pin_up[i]
        ws.cell(2 + i, 4).value = pin_down[i]
    wb.save(os.path.join(target_divide, filename[:-3] + "xlsx"))
"""


def clean_light(img_light_pth):
    """
        亮度图清洗，去除黑色部分，只保留工件
        必须传入包括文件名的文件路径
        Args:
            img_name: image name with file path
            imshow:
            imsave:
            target_folder: folder to save image

        Returns:
            rgb image
        """
    img_light = cv2.imread(img_light_pth)
    if img_light_pth.count("_side_") > 0:
        img_light = img_light[:2900, :1000, :]
    else:
        img_light = img_light[:1700, :]
    """
    clean pic
    """
    img_gray = cv2.cvtColor(img_light, cv2.COLOR_BGR2GRAY)
    row, column = img_gray.shape
    left = 0
    right = 0
    for i in range(row):
        if np.sum(img_gray[i, :]) != 0:
            left = i
            break
    for i in range(row - 1, -1, -1):
        if np.sum(img_gray[i, :]) != 0:
            right = i + 1
            break
    img_gray = img_gray[left:right, :]
    img_light = img_light[left:right, :, :]
    row, column = img_gray.shape
    for i in range(column):
        if np.sum(img_gray[:, i]) != 0:
            left = i
            break
    for i in range(column - 1, -1, -1):
        if np.sum(img_gray[:, i]) != 0:
            right = i + 1
            break
    img_light = img_light[:, left:right, :]
    # print(img_light.shape)
    return img_light


def clean_height(npy_height_pth, nan_num=np.nan):
    """
    Auth: WZW
    根据nan，提取周围部分，并将剩余的nan数据转为nan_num
    UpArea: [ :1500, : ]
    SideArea: [ :2900, :1000]
    PreVersion: clean_height in V1
    Args:
        image: np.array
        nan_num:
    Returns:
        compressed data
    """
    npy_height = np.load(npy_height_pth)
    if npy_height_pth.count("_side_") > 0:
        image = npy_height[:2900, :1000]
    else:
        image = npy_height[:1700, :]
    image = clean_edge(image)
    # print(image.shape)
    if not np.isnan(nan_num):
        print("数据压缩，修改nan为：" + str(nan_num))
        nan_index = np.isnan(image)
        image[nan_index] = nan_num
    npy_height = image
    return npy_height


def clean_edge(npy):
    """
    清理图像边缘，方便后续切片
    Args:
        npy:

    Returns:
    """
    left = 0
    right = 0
    row, column = npy.shape
    for i in range(row):
        if len(npy[i, np.isnan(npy[i, :])]) != column:
            left = i
            break
    for i in range(row - 1, -1, -1):
        if len(npy[i, np.isnan(npy[i, :])]) != column:
            right = i + 1
            break
    npy = npy[left:right, :]
    row, column = npy.shape
    left = 0
    right = 0
    for i in range(column):
        if len(npy[np.isnan(npy[:, i]), i]) != row:
            left = i
            break
    for i in range(column - 1, -1, -1):
        if len(npy[np.isnan(npy[:, i]), i]) != row:
            right = i + 1
            break
    npy = npy[:, left:right]
    return npy


def cal_img(path):
    # cv2.namedwindow("img", cv2.WINDOW_NORMAL)
    cv2.namedWindow("img", cv2.WINDOW_KEEPRATIO)
    img = cv2.imread(path)
    # img =
    # print(img.shape)
    cv2.imshow("img", img)
    cv2.waitKey(0)


def read_3d_npy(z_points):
    """
    显示npy格式的三维点云数据
    :param npy_file: npy文件路径
    :return: o3d.geometry.PointCloud
    """

    # z_points = np.load(npy_file)
    y_num, x_num = z_points.shape

    unit_x_point = np.arange(x_num)
    x_points = np.tile(unit_x_point, y_num) / RADIO

    unit_y_point = np.arange(y_num)
    y_points = np.repeat(unit_y_point, x_num) / RADIO

    z_points = z_points.reshape((-1))

    nan_index = np.isnan(z_points)

    # remove nan points
    x_points = x_points[~nan_index]
    y_points = y_points[~nan_index]
    z_points = z_points[~nan_index]
    pcd = o3d.geometry.PointCloud()
    m3d = np.array([x_points, y_points, z_points]).T
    pcd.points = o3d.utility.Vector3dVector(m3d)
    return pcd, x_points, y_points, z_points


def read_3d_npy_add_background(z_points, depth=-10):
    """
    显示npy格式的三维点云数据
    :param npy_file: npy文件路径
    :return: o3d.geometry.PointCloud
    """

    # z_points = np.load(npy_file)
    y_num, x_num = z_points.shape

    unit_x_point = np.arange(x_num)
    x_points = np.tile(unit_x_point, y_num) / RADIO
    x_back = x_points.copy()

    unit_y_point = np.arange(y_num)
    y_points = np.repeat(unit_y_point, x_num) / RADIO
    y_back = y_points.copy()

    z_points = z_points.reshape((-1))
    z_back = np.ones(x_back.shape) * depth
    print(z_back.shape)

    nan_index = np.isnan(z_points)

    # remove nan points
    x_points = x_points[~nan_index]
    y_points = y_points[~nan_index]
    z_points = z_points[~nan_index]
    x_points = np.append(x_points, x_back)
    y_points = np.append(y_points, y_back)
    z_points = np.append(z_points, z_back)

    pcd = o3d.geometry.PointCloud()
    m3d = np.array([x_points, y_points, z_points]).T
    pcd.points = o3d.utility.Vector3dVector(m3d)
    return pcd, x_points, y_points, z_points


def vis_pcd(pcd, down_sample=True):
    if down_sample:
        voxel_down_pcd = pcd.voxel_down_sample(voxel_size=0.02)
        o3d.visualization.draw_geometries([voxel_down_pcd])
    else:
        o3d.visualization.draw_geometries([pcd])


def filter_pin_npy(npy, mx=0, mn=-3.5):
    """
    根据mx和mn提取出侧面pin针数据
    包括上下两部分
    Args:
        npy:
        mx:
        mn:

    Returns:

    """
    index_0 = npy < mx
    index_1 = npy > mn
    index = np.logical_not(np.logical_and(index_0, index_1))
    npy[index] = np.nan
    return npy


def get_slice_num(npy, mx, mn):
    """

    Args:
        npy:
        mx: 切片的最大值
        mn: 切片的最小值
    Returns: 切片中的点数

    """
    index_0 = npy <= mx
    index_1 = npy > mn
    index = np.logical_and(index_0, index_1)
    # a = np.sum(index_1)
    # b = np.sum(index_0)
    return np.sum(index)
    # print(index.shape)


def cut_slice_visual(npy, npy_up, npy_down, pin, step=0.0125):
    step = np.float64(step)
    index_nan = np.isnan(npy)
    min_depth = npy[~index_nan].min()
    max_depth = npy[~index_nan].max()
    depth_result = 100
    x = np.arange(min_depth - step, max_depth + step, step)
    test_npy = npy.copy()
    add = 0
    for i in np.arange(min_depth - step, max_depth + step, step):
        index = test_npy > i + add
        test_npy[index] = test_npy[index] + 0.5
        add = add + 0.5
    vis_data, x, y, z = read_3d_npy(test_npy)
    vis_pcd(vis_data)


def cut_slice(npy, npy_up, npy_down, pin, step=0.0125):
    step = np.float64(step)
    index_nan = np.isnan(npy)
    min_depth = npy[~index_nan].min()
    max_depth = npy[~index_nan].max()
    depth_result = 100
    for i in np.arange(min_depth - step, max_depth + step, step):
        if get_slice_num(npy, i + step, i) > 50:
            index_0 = npy <= i + step
            index_1 = npy > i
            index_10 = np.logical_and(index_0, index_1)
            depth_result = np.mean(npy[index_10])
            break
    # result = []
    # result_up = []
    # result_down = []
    # # back2front
    # # x = np.arange(min_depth - step, max_depth + step, step)
    # # for i in np.arange(min_depth - step, max_depth + step, step):
    # #     result.append(get_slice_num(npy, i + step, i))
    # #     result_up.append(get_slice_num(npy_up, i + step, i))
    # #     result_down.append(get_slice_num(npy_down, i + step, i))
    # # front2back
    # # x = np.arange(max_depth + step, min_depth - 2 * step, -step)
    # # for i in np.arange(max_depth + step, min_depth - 2 * step, -step):
    # #     result.append(get_slice_num(npy, i, i - step))
    # #     result_up.append(get_slice_num(npy_up, i, i - step))
    # #     result_down.append(get_slice_num(npy_down, i, i - step))
    # # 裁剪点
    # x = np.arange(min_depth - step, max_depth + step, step)
    # for i in np.arange(min_depth - step, max_depth + step, step):
    #     result.append(get_slice_num(npy, i + step, i))
    #     result_up.append(get_slice_num(npy_up, i + step, i))
    #     result_down.append(get_slice_num(npy_down, i + step, i))
    # # write_slice(result, result_up, result_down, np.arange(min_depth - step, max_depth + step, step))
    # # print(result)
    # # print(result_down)
    # # print(result_up)
    # y = range(2000)
    # z = np.ones(2000) * depth_result
    # plt.cla()
    # r1, = plt.plot(x, result, label='total')
    # r2, = plt.plot(x, result_up, label='up')
    # r3, = plt.plot(x, result_down, label='down')
    # r4, = plt.plot(z, y, label='back')
    # plt.legend(handles=[r1, r2, r3, r4], labels=['total', 'up', 'down', 'back'], loc='best')
    # # plt.show()
    # # plt.savefig(os.path.join("D:/Programs/侧面体积结果/pin底切割线",
    # #                          "3505277134ML085B085301110012200514_side_2022-06-28-09-07-46" + "_pin" + str(
    # #                              pin) + ".png"))
    # # plt.savefig(os.path.join("D:/Programs/侧面体积结果/pin底切割线", filename[:-4] + "_pin" + str(pin) + ".png"))
    # # plt.savefig(os.path.join("D:/Programs/侧面体积结果/pin针切片点数曲线", filename[:-4] + "_pin" + str(pin) + "_back.png"))
    # # plt.savefig(os.path.join("D:/Programs/侧面体积结果/pin针切片点数曲线", filename[:-4] + "_pin" + str(pin) + ".png"))
    return depth_result


def divide_up_side_pin(npy_data):
    """
    分离侧面点云数据的上部和下部
    Args:
        npy_data:

    Returns:
        pin_down, pin_up
    """
    start = time.time()
    npy = clean_edge(npy_data)
    y_num, x_num = npy.shape
    index = np.isnan(npy)
    left = -1
    right = -1
    for i in range(x_num):
        if (np.sum(index[:, i]) == y_num):
            if left == -1:
                left = i
            else:
                right = i + 1
    end = time.time()
    print("分离上下pin时间: ", end - start)
    # clean_reflection(npy[:, :left])
    # vis_data, x, y, z = read_3d_npy(npy[:, :left])
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy(npy[:, right:])
    # vis_pcd(vis_data)
    return npy[:, :left], npy[:, right:]


def confirm_reflection(area, min_ind):
    for i in range(0, min_ind):
        if area[i] < area[i + 1]:
            return False
    for i in range(min_ind, min_ind + 2):
        if area[i] > area[i + 1]:
            return False
    return True


def clean_reflection(npy, fileName=None):
    """
    对于第一个pin，在边缘总是会有一部分反射干扰
    用于清理干扰
    Args:
        npy:

    Returns: 反射结束的点

    """
    try:
        npp = npy.copy()
        mean = np.nanmean(npp, axis=1)
        max_index = 60 + np.where(mean[60:100] == np.nanmax(mean[60:100]))[0][-1]
        index = np.where(mean[:max_index] == np.nanmin(mean[:max_index]))
        # index = mean[:100].index(np.nanmin(mean[:100]))
        index = index[0][-1]
        print("=== index", index)
        if (index >= 2):
            if not confirm_reflection(mean[index - 2:index + 3], 2):
                index = 0
        else:
            if not confirm_reflection(mean[0:index + 3], index):
                index = 0
        if not (fileName is None):
            path, fin = os.path.split(fileName)
            fname, fsuffix = os.path.splitext(fin)
            plt.cla()
            plt.plot(mean)
            plt.axvline(x=index)
            plt.axhline(y=mean[index])
            plt.savefig(os.path.join("D:\\Programs\\data\\exp\\分离反射找转折点\\20220905", fname + ".png"))
            print(fileName[:-3] + "png")
            # vis_data, x, y, z = read_3d_npy(npy[index:])
            # vis_pcd(vis_data)
        return index
    except Exception as e:
        print("=== exception, ", e, " index: ", 0)
        return 0


def clean_discrete_points(z_points, epoch=1):
    """
    过滤离散点
    Args:
        npy:
        epoch: 过滤轮次
    Returns:
        result: 消除离散点后的结果

    """
    # z_points = np.load(npy_file)
    y_num, x_num = z_points.shape
    result = np.full(z_points.shape, np.nan)
    # print(z_points.shape)
    # print(result.shape)
    unit_x_point = np.arange(x_num)
    x_points = np.tile(unit_x_point, y_num)

    unit_y_point = np.arange(y_num)
    y_points = np.repeat(unit_y_point, x_num)

    z_points = z_points.reshape((-1))

    nan_index = np.isnan(z_points)

    # remove nan points
    x_points = x_points[~nan_index]
    y_points = y_points[~nan_index]
    z_points = z_points[~nan_index]
    # 50937
    for i in range(epoch):
        x_points, y_points, z_points = calculate_utils.denoising_by_remove_points(x_points, y_points, z_points,
                                                                                  show=False)
    # 50257
    x_points = np.asarray(x_points, dtype=int)
    y_points = np.asarray(y_points, dtype=int)
    for i in range(x_points.shape[0]):
        result[y_points[i], x_points[i]] = z_points[i]
    return result


def extract_valid_area(npy_data):
    """
    提取有效pin针区域
    Args:
        npy_data:

    Returns:
        area: pin的有效区域

    """
    npy = npy_data.copy()
    y_num, x_num = npy.shape
    left = -1
    right = -1
    area = []
    for i in range(y_num):
        if len(npy[i, np.isnan(npy[i, :])]) != x_num:
            if left == -1:
                left = i
            right = i + 1
        elif left != -1:
            if right - left < 80:
                npy[left:right, :] = np.nan
            elif np.sum(~np.isnan(npy[left:right, :])) > 10000:
                area.append((left, right, np.sum(~np.isnan(npy[left:right, :]))))
            left = -1
            right = -1
    del npy
    area.sort(key=lambda x: x[2], reverse=True)
    result = area[:3]
    result.sort()
    print(result)
    return result


def get_real_side_depth(npy, depth, area=0.5, step=0.0125):
    max_points = 0
    real_depth = -100
    for i in np.arange(depth - step, depth + area + step, step):
        num = get_slice_num(npy, i + step, i)
        if num > max_points:
            real_depth = i - step
            max_points = num
            if max_points > 800:
                break
    return real_depth


def calculate_side_volumn_pin(z, z_min):
    aim_index = z > z_min
    z_points = z[aim_index] - z_min
    return np.sum(z_points) * CONSTANT_SIDE


def calculate_side_pin(npy_path):
    # 清理nan数据
    npy_data = clean_height(npy_path)
    # vis_data, x, y, z = read_3d_npy(npy_data)
    # vis_pcd(vis_data)
    # npy_data = filter_pin_npy(npy_data, -1, -3.5)
    npy_data = filter_pin_npy(npy_data, 2.3, -1)
    vis_data, x, y, z = read_3d_npy(npy_data)
    vis_pcd(vis_data)
    sta = time.time()
    # npy_data = clean_discrete_points(npy_data, 2)
    # 提取pin针
    area = extract_valid_area(npy_data)
    pin1 = npy_data[area[0][0]:area[0][1], :]
    pin2 = npy_data[area[1][0]:area[1][1], :]
    pin3 = npy_data[area[2][0]:area[2][1], :]
    # 清理离散点
    pin1 = clean_discrete_points(pin1, 2)
    pin2 = clean_discrete_points(pin2, 2)
    pin3 = clean_discrete_points(pin3, 2)
    # pin针上下分离
    pin1_down, pin1_up = divide_up_side_pin(pin1)
    pin2_down, pin2_up = divide_up_side_pin(pin2)
    pin3_down, pin3_up = divide_up_side_pin(pin3)
    # pin针反射值清理
    index = clean_reflection(pin1_down)

    # pin_total_nan = np.full([50, pin1.shape[1]], np.nan)
    # pin1_nan_add = np.vstack((pin1[:index, :], pin_total_nan))
    # pin1_nan_add = np.vstack((pin1_nan_add, pin1[index:, :]))

    pin1 = pin1[index:, :]
    pin1_down = pin1_down[index:, :]
    pin1_up = pin1_up[index:, :]

    index = clean_reflection(pin2_down)

    # pin_total_nan = np.full([50, pin2.shape[1]], np.nan)
    # pin2_nan_add = np.vstack((pin2[:index, :], pin_total_nan))
    # pin2_nan_add = np.vstack((pin2_nan_add, pin2[index:, :]))

    pin2 = pin2[index:, :]
    pin2_down = pin2_down[index:, :]
    pin2_up = pin2_up[index:, :]

    index = clean_reflection(pin3_down)

    # pin_total_nan = np.full([50, pin3.shape[1]], np.nan)
    # pin3_nan_add = np.vstack((pin3[:index, :], pin_total_nan))
    # pin3_nan_add = np.vstack((pin3_nan_add, pin3[index:, :]))

    pin3 = pin3[index:, :]
    pin3_down = pin3_down[index:, :]
    pin3_up = pin3_up[index:, :]

    # 统计有效点个数
    # pin1_up_num.append(np.sum(~np.isnan(pin1_up)))
    # pin1_down_num.append(np.sum(~np.isnan(pin1_down)))
    # pin2_up_num.append(np.sum(~np.isnan(pin2_up)))
    # pin2_down_num.append(np.sum(~np.isnan(pin2_down)))
    # pin3_up_num.append(np.sum(~np.isnan(pin3_up)))
    # pin3_down_num.append(np.sum(~np.isnan(pin3_down)))
    # pin1_sum.append(np.sum(~np.isnan(pin1)))
    # pin2_sum.append(np.sum(~np.isnan(pin2)))
    # pin3_sum.append(np.sum(~np.isnan(pin3)))

    # 粗略估计pin针深度
    depth_pin1 = cut_slice(pin1, pin1_up, pin1_down, 1)
    depth_pin2 = cut_slice(pin2, pin2_up, pin2_down, 2)
    depth_pin3 = cut_slice(pin3, pin3_up, pin3_down, 3)

    # 获得精确深度
    real_pin1 = get_real_side_depth(pin1, depth_pin1)
    real_pin2 = get_real_side_depth(pin1, depth_pin2)
    real_pin3 = get_real_side_depth(pin1, depth_pin3)

    depth_pin1 = min(depth_pin1 + 0.2, real_pin1)
    depth_pin2 = min(depth_pin2 + 0.2, real_pin2)
    depth_pin3 = min(depth_pin2 + 0.2, real_pin3)

    # 计算pin针体积
    pin1_val = calculate_side_volumn_pin(pin1_down, depth_pin1)
    pin2_val = calculate_side_volumn_pin(pin2_down, depth_pin2)
    pin3_val = calculate_side_volumn_pin(pin3_down, depth_pin3)
    # write_vol(pin1_val, pin2_val, pin3_val)
    end = time.time()
    print("total time: ", end - sta)
    print(pin1_val, pin2_val, pin3_val)

    # vis_data, x, y, z = read_3d_npy(pin1_nan_add)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy(pin2_nan_add)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy(pin3_nan_add)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy_add_background(pin1_nan_add, depth_pin1)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy_add_background(pin2_nan_add, depth_pin2)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy_add_background(pin3_nan_add, depth_pin3)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy_add_background(pin1, depth_pin1)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy_add_background(pin2, depth_pin2)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy_add_background(pin3, depth_pin3)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy(pin1)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy(pin2)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy(pin3)
    # vis_pcd(vis_data)


def cal_side_volume(npy_pth):
    # 清理nan数据
    npy_data = clean_height(npy_pth)
    vis_data, x, y, z = read_3d_npy(npy_data)
    # vis_pcd(vis_data)
    npy_data = filter_pin_npy(npy_data, 2.5, -1)
    vis_data, x, y, z = read_3d_npy(npy_data)
    # vis_pcd(vis_data)

    # sta = time.time()
    # npy_data = clean_discrete_points(npy_data, 2)
    # 提取pin针
    area = extract_valid_area(npy_data)
    pin1 = npy_data[area[0][0]:area[0][1], :]
    pin2 = npy_data[area[1][0]:area[1][1], :]
    pin3 = npy_data[area[2][0]:area[2][1], :]
    # vis_pcd(read_3d_npy(pin1)[0])
    # vis_pcd(read_3d_npy(pin2)[0])
    # vis_pcd(read_3d_npy(pin3)[0])

    # 清理离散点
    pin1 = clean_discrete_points(pin1, 2)
    pin2 = clean_discrete_points(pin2, 2)
    pin3 = clean_discrete_points(pin3, 2)

    # pin针上下分离
    pin1_down, pin1_up = divide_up_side_pin(pin1)
    pin2_down, pin2_up = divide_up_side_pin(pin2)
    pin3_down, pin3_up = divide_up_side_pin(pin3)

    # pin针反射值清理
    # index = clean_reflection(pin1_down)
    # pin1 = pin1[index:, :]
    # pin1_down = pin1_down[index:, :]
    # pin1_up = pin1_up[index:, :]
    # index = clean_reflection(pin2_down)
    # pin2 = pin2[index:, :]
    # pin2_down = pin2_down[index:, :]
    # pin2_up = pin2_up[index:, :]
    # index = clean_reflection(pin3_down)
    # pin3 = pin3[index:, :]
    # pin3_down = pin3_down[index:, :]
    # pin3_up = pin3_up[index:, :]

    # test = np.ones(npy_data.shape) * np.nan
    # test[area[0][0]:area[0][1], :] = pin1
    # test[area[1][0]:area[1][1], :] = pin2
    # test[area[2][0]:area[2][1], :] = pin3
    #
    # vis_data, x, y, z = read_3d_npy(test)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy(pin1)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy(pin2)
    # vis_pcd(vis_data)
    # vis_data, x, y, z = read_3d_npy(pin3)
    # vis_pcd(vis_data)

    # 粗略估计pin针深度
    depth_pin1 = cut_slice(pin1, pin1_up, pin1_down, 1)
    depth_pin2 = cut_slice(pin2, pin2_up, pin2_down, 2)
    depth_pin3 = cut_slice(pin3, pin3_up, pin3_down, 3)

    # 获得精确深度
    real_pin1 = get_real_side_depth(pin1, depth_pin1)
    real_pin2 = get_real_side_depth(pin1, depth_pin2)
    real_pin3 = get_real_side_depth(pin1, depth_pin3)
    depth_pin1 = min(depth_pin1 + 0.2, max(depth_pin1, real_pin1))
    depth_pin2 = min(depth_pin2 + 0.2, max(depth_pin1, real_pin2))
    depth_pin3 = min(depth_pin2 + 0.2, max(depth_pin1, real_pin3))

    # 计算pin针体积
    pin1_val = calculate_side_volumn_pin(pin1_down, depth_pin1)
    pin2_val = calculate_side_volumn_pin(pin2_down, depth_pin2)
    pin3_val = calculate_side_volumn_pin(pin3_down, depth_pin3)
    # write_vol(pin1_val, pin2_val, pin3_val)
    print(pin1_val, pin2_val, pin3_val)
    return pin1_val, pin2_val, pin3_val


if __name__ == "__main__":
    """
    """
    # img_dir = "D:\\Programs\\3D-AI-check\\test_data\\3505277134ML085B085301110012200465_side_2022-06-28-09-05-21_meta.png"
    # npy_dir = "D:\\Programs\\3D-AI-check\\test_data\\3505277134ML085B085301110012200465_side_2022-06-28-09-05-21.npy"
    # npy_dir = "D:/Programs/data/dataset/20220701-0628-full-size/2022-06-28/3505277134ML085B085301110012200514_side_2022-06-28-09-07-46.npy"
    # npy_dir = "D:/Programs/data/dataset/20220701-0628-full-size/2022-06-28\\3505277134ML085B085301110012200685_side_2022-06-28-08-23-09.npy"
    # npy_dir = "D:/Programs/data/dataset/20220701-0628-full-size/2022-06-28/3505277134ML085B085301110012200542_side_2022-06-28-08-20-44.npy"
    # npy_dir = "D:/Programs/data/dataset/20220701-0628-full-size/2022-06-28/3505277134ML085B085301110012200465_side_2022-06-28-09-05-21.npy"
    # npy_dir = "D:/Programs/data/dataset/20220701-0628-full-size/2022-06-28/35051772623505277134A28062200152544_side_2022-06-28-16-53-37.npy"
    # npy_dir = "D:/Programs/data/dataset/20220701-0628-full-size/2022-06-28/3505277134ML085B085301110012200465_side_2022-06-28-09-05-21.npy"
    # npy_dir = "D:/Programs/data/dataset/20220701-0628-full-size/2022-06-28/3505277134ML085B085301110012200485_side_2022-06-28-08-54-13.npy"
    # calculate_side_pin(npy_dir)
    # # cal_img(img_dir)
    # # data = clean_light(img_dir)
    # # cv2.namedWindow("img", cv2.WINDOW_NORMAL)
    # # # img =
    # # x, y = data.shape[:2]
    # # cv2.imshow("img", data)
    # # cv2.namedWindow("pin1", cv2.WINDOW_NORMAL)
    # # cv2.namedWindow("pin2", cv2.WINDOW_NORMAL)
    # # cv2.namedWindow("pin3", cv2.WINDOW_NORMAL)
    # # pin1 = data[430:730, :, :]
    # # pin2 = data[1250:1570, :, :]
    # # pin3 = data[2100:2400, :, :]
    # # cv2.imshow("pin1", pin1)
    # # cv2.imshow("pin2", pin2)
    # # cv2.imshow("pin3", pin3)
    # # cv2.waitKey(0)
    #
    # data = clean_height(npy_dir)
    # pin1 = data[400:760, :]
    # pin2 = data[1200:1650, :]
    # pin3 = data[2050:2400, :]
    # pin1 = filter_pin_npy(pin1, 0, -5)
    # pin2 = filter_pin_npy(pin2, 0, -5)
    # pin3 = filter_pin_npy(pin3, 0, -5)
    # pin1 = clean_edge(pin1)
    # pin2 = clean_edge(pin2)
    # pin3 = clean_edge(pin3)
    # print(pin1.shape)
    # # 取顶端
    # # pin1 = pin1[100:250, 590:630]
    # # pin2 = pin2[100:250, 590:630]
    # # pin3 = pin3[100:250, 590:630]
    # # 取上部pin
    # # pin1 = pin1[:, 380:]
    # # pin2 = pin2[:, 380:]
    # # pin3 = pin3[:, 380:]
    # # 取下部pin
    # # pin1 = pin1[:, :380]
    # # pin2 = pin2[:, :380]
    # # pin3 = pin3[:, :380]
    # # 430:730
    # # 1250:1570
    # # 2100:2400
    # """
    # 切片测试
    # """
    # cut_slice(pin1, 0, -5)
    # sta = time.time()
    # pin1 = clean_discrete_points(pin1, 2)
    # end = time.time()
    # print("time: ", end - sta)
    # p1, x, y, z = read_3d_npy(pin1)
    # # p2, x, y, z = read_3d_npy(pin2)
    # # p3, x, y, z = read_3d_npy(pin3)
    # vis_pcd(p1)
    # # vis_pcd(p2)
    # # vis_pcd(p3)
    # """
    # """
    # pin1_down_num = []
    # pin1_up_num = []
    # pin2_down_num = []
    # pin2_up_num = []
    # pin3_down_num = []
    # pin3_up_num = []
    # pin1_sum = []
    # pin2_sum = []
    # pin3_sum = []
    cal_side_volume(
        "D:\\Programs\\data\\dataset\\20220831\\35051772633505277134A31082200188947_side_2022-08-31-10-33-55.npy")
    root = "D:/Programs/data/dataset/20220701-0628-full-size/2022-06-28"
    root = "D:\\Programs\\data\\dataset\\20220831"
    file = os.listdir(root)
    target_divide = "D:/Programs/侧面体积结果/csv/分割点列"
    vol_folder = "D:/Programs/侧面体积结果/csv/体积.xlsx"
    for filename in file:
        if filename[-3:] == 'npy' and filename.count("_side_") > 0:
            print(os.path.join(root, filename))
            # calculate_side_pin(os.path.join(root, filename))
            cal_side_volume(os.path.join(root, filename))
    # plt.cla()
    # plt.plot(pin1_up_num)
    # plt.savefig("D:/Programs/侧面体积结果/pin针面积估计/pin1_up_num.png")
    # plt.cla()
    # plt.plot(pin1_down_num)
    # plt.savefig("D:/Programs/侧面体积结果/pin针面积估计/pin1_down_num.png")
    # plt.cla()
    # plt.plot(pin2_up_num)
    # plt.savefig("D:/Programs/侧面体积结果/pin针面积估计/pin2_up_num.png")
    # plt.cla()
    # plt.plot(pin2_down_num)
    # plt.savefig("D:/Programs/侧面体积结果/pin针面积估计/pin2_down_num.png")
    # plt.cla()
    # plt.plot(pin3_up_num)
    # plt.savefig("D:/Programs/侧面体积结果/pin针面积估计/pin3_up_num.png")
    # plt.cla()
    # plt.plot(pin3_down_num)
    # plt.savefig("D:/Programs/侧面体积结果/pin针面积估计/pin3_down_num.png")
    # plt.cla()
    # plt.plot(pin1_sum)
    # plt.savefig("D:/Programs/侧面体积结果/pin针面积估计/pin1_sum.png")
    # plt.cla()
    # plt.plot(pin2_sum)
    # plt.savefig("D:/Programs/侧面体积结果/pin针面积估计/pin2_sum.png")
    # plt.cla()
    # plt.plot(pin3_sum)
    # plt.savefig("D:/Programs/侧面体积结果/pin针面积估计/pin3_sum.png")
    # data = clean_height(os.path.join(root, i))
    # pin1 = data[400:760, :]
    # pin2 = data[1200:1650, :]
    # pin3 = data[2050:2400, :]
    # pin1 = filter_pin_npy(pin1, 0, -5)
    # pin2 = filter_pin_npy(pin2, 0, -5)
    # pin3 = filter_pin_npy(pin3, 0, -5)
    # pin1 = clean_edge(pin1)
    # pin2 = clean_edge(pin2)
    # pin3 = clean_edge(pin3)
    # p1, x, y, z = read_3d_npy(pin1)
    # p2, x, y, z = read_3d_npy(pin2)
    # p3, x, y, z = read_3d_npy(pin3)
    # vis_pcd(p1)
    # vis_pcd(p2)
    # vis_pcd(p3)
